#!/usr/bin/env python3
#coding=utf-8

import rospy
from std_msgs.msg import String
import json
import paho.mqtt.client as mqtt
import time
from qq_msgs.msg import Carry

class ROStoMQTTBridge:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('mqtt_bridge', anonymous=True)
        
        # MQTT连接参数
        self.client = mqtt.Client(protocol=mqtt.MQTTv311)
        self.client.username_pw_set(username='root', password='NZ852ijd169@')
        
        # 设置回调函数
        self.client.on_connect = self.on_connect
        self.client.on_disconnect = self.on_disconnect
        
        # 连接 MQTT 服务器
        try:
            self.client.connect('81.70.146.224', 1883, 60)
            self.client.loop_start()
        except Exception as e:
            rospy.logerr(f"Failed to connect to MQTT server: {str(e)}")
            return
            
        # 订阅两个ROS话题
        rospy.Subscriber('/kaiheiqun', Carry, self.kaiheiqun_callback)
        rospy.Subscriber('/qiudaiqun', String, self.qiudaiqun_callback)
        
        # 设置发布频率为1Hz
        self.rate = rospy.Rate(1)
        self.last_publish_time = 0  # 记录上次发布的时间
        
        # 存储最新消息
        self.latest_kaiheiqun = None
        self.latest_qiudaiqun = None
        
        rospy.loginfo("ROS to MQTT bridge initialized")

    def on_connect(self, client, userdata, flags, rc):
        if rc == 0:
            rospy.loginfo("Connected to MQTT server")
        else:
            rospy.logerr(f"Failed to connect with code: {rc}")

    def on_disconnect(self, client, userdata, rc):
        rospy.logwarn(f"Disconnected from MQTT server with code: {rc}")

    def kaiheiqun_callback(self, data):
        # 存储最新的 /kaiheiqun 消息
        self.latest_kaiheiqun = data
        self.publish_if_ready()

    def qiudaiqun_callback(self, data):
        # 存储最新的 /qiudaiqun 消息
        self.latest_qiudaiqun = data
        self.publish_if_ready()

    def publish_if_ready(self):
        current_time = time.time()
        # 检查是否距离上次发布超过1秒，并且两个话题都有数据
        if (current_time - self.last_publish_time >= 1.0 and 
            self.latest_kaiheiqun is not None and 
            self.latest_qiudaiqun is not None):
            # 创建组合的JSON数据
            json_data = {
                "identifier": 159951,
                "kaiheiqun": {
                    "grate": self.latest_kaiheiqun.grate,
                    "star": self.latest_kaiheiqun.star,
                    "data": self.latest_kaiheiqun.data
                },
                "qiudaiqun": {
                    "message": self.latest_qiudaiqun.data
                },
                "timestamp": current_time
            }
            
            # 转换为JSON字符串
            json_str = json.dumps(json_data)
            
            # 通过MQTT发布
            try:
                self.client.publish('nzzn/test/0', json_str)
                rospy.loginfo(f"Published to MQTT: {json_str}")
                self.last_publish_time = current_time  # 更新上次发布时间
            except Exception as e:
                rospy.logerr(f"Failed to publish to MQTT: {str(e)}")

    def run(self):
        while not rospy.is_shutdown():
            self.rate.sleep()

    def shutdown(self):
        self.client.loop_stop()
        self.client.disconnect()
        rospy.loginfo("MQTT client disconnected")

if __name__ == '__main__':
    try:
        bridge = ROStoMQTTBridge()
        bridge.run()
    except rospy.ROSInterruptException:
        pass
    finally:
        if 'bridge' in locals():
            bridge.shutdown()